#include <stdio.h>
#include <math.h>
using namespace std;

#include "fish_eye_restorer_function_lut.h"

FishEyeRestorerFunctionLUT::FishEyeRestorerFunctionLUT
   (float fisheye_len_field_of_view,
    int fisheye_len_image_width,
    int fisheye_len_image_height,
    int restored_image_width,
    int restored_image_height) {

  LUT_dim_i = restored_image_height;
  LUT_dim_j = restored_image_width;

  // Aloca espaço para a LUT
  function_LUT = new LUT_element*[LUT_dim_i];
  for (int i = 0;  i < LUT_dim_i; ++ i) {
    function_LUT[i] = new LUT_element[LUT_dim_j];
  }

  // Coordenadas de Oc no sistema r
  float Oc_in_Of_x = fisheye_len_image_height / 2 + 35;
  float Oc_in_Of_y = fisheye_len_image_width / 2 - 10;

  // Coordenadas de Oc no sistema f
  float Oc_in_Or_x = restored_image_height / 2;
  float Oc_in_Or_y = restored_image_width / 2;

  float f = (1283) / (4 * sin(fisheye_len_field_of_view / 2));

  for (int i = 0; i < restored_image_height; ++i) {
    for (int j = 0; j < restored_image_width; ++j) {
      // Coordenadas do "centro" do PixelCoordinates p no sistema r
      float p_in_Or_x = i + 0.5;
      float p_in_Or_y = j + 0.5;

      // Coordenadas do "centro" do PixelCoordinates p no sistema c
      float p_in_Oc_x = p_in_Or_x - Oc_in_Or_x;
      float p_in_Oc_y = p_in_Or_y - Oc_in_Or_y;

      float lambda = (sqrt(p_in_Oc_x*p_in_Oc_x + p_in_Oc_y*p_in_Oc_y))/f;
      float factor = (lambda == 0) ? 1 : 2*((sin(atan(lambda)/2))/lambda);

      // Coordenadas do "centro" do PixelCoordinates f no sistema c
      float f_in_Oc_x = p_in_Oc_x * factor;
      float f_in_Oc_y = p_in_Oc_y * factor;

      // Coordenadas do "centro" do PixelCoordinates f no sistema f
      float f_in_Of_x = f_in_Oc_x + Oc_in_Of_x;
      float f_in_Of_y = f_in_Oc_y + Oc_in_Of_y;

      // Coordenadas discretas do "centro" do PixelCoordinates f no sistema c
      int f_in_Of_i = f_in_Of_x;
      int f_in_Of_j = f_in_Of_y;

      LUT_element *current = &function_LUT[i][j];
      if (f_in_Of_i >= 0 && f_in_Of_j >= 0 &&
        f_in_Of_i < fisheye_len_image_height &&
        f_in_Of_j < fisheye_len_image_width) {
        current->point.x = f_in_Of_i;
        current->point.y = f_in_Of_j;
        current->valid = true;
      } else {
        current->valid = false;
      }
    }
  }
}

PixelCoordinates *FishEyeRestorerFunctionLUT::AccessLut(int i, int j) {
  LUT_element calculated;
  PixelCoordinates *result = NULL;

  // Programação defensiva!
  if (i >= 0 && j >= 0 && i < LUT_dim_i && j < LUT_dim_j) {
    calculated = function_LUT[i][j];
    if (calculated.valid) result = &(calculated.point);
  }

  return result;
}
